#pragma once
#include <string.h>
#include <cstdint>
#include <vector>
#include <string>
#include "libnml/posemath/posemath.h"

const size_t MAX_AXIS = 9;

const size_t MAX_GROUP = 7;

#define DEG_TO_RAD 0.017453292519943295
#define ROBOT_JOINT_DEG_TO_RAD(q_rad, q_deg)                                              \
    do {                                                                                  \
        for (size_t i = 0; i < q_deg.MAX_AXIS; i++) { q_rad[i] = q_deg[i] * DEG_TO_RAD; } \
    } while (0)

typedef struct ZucPose
{
    PmCartesian tran;
    double a, b, c;
    double u, v, w;
    std::string get_zucpos_str()
    {
        std::string str = "";
        str += "[" + std::to_string(static_cast<double>(tran.x));
        str += " ,";
        str += std::to_string(static_cast<double>(tran.y));
        str += " ,";
        str += std::to_string(static_cast<double>(tran.z));
        str += " ,";
        str += std::to_string(static_cast<double>(a));
        str += " ,";
        str += std::to_string(static_cast<double>(b));
        str += " ,";
        str += std::to_string(static_cast<double>(c));
        str += "]\n";
        return str;
    }
} ZucPose;
template <typename T> class RobJointData
{
public:
    static const size_t MAX_AXIS = 9;
    RobJointData() { memset(val, 0, MAX_AXIS * sizeof(T)); }
    T& operator[](size_t idx) { return val[idx]; }

    T val[MAX_AXIS];
    std::string val_str() const
    {
        std::string res = "";
        for (size_t i = 0; i < MAX_AXIS; i++) { res += std::to_string(static_cast<double>(val[i])) + ","; }
        return res;
    }
};

class RobJointVal : public RobJointData<double>
{
public:
    RobJointVal() : RobJointData<double>() {}
    RobJointVal(const double v[MAX_AXIS])
    {
        for (size_t i = 0; i < MAX_AXIS; i++) { val[i] = v[i]; }
    }
    RobJointVal(std::vector<double> v)
    {
        size_t minsize = v.size() < MAX_AXIS ? v.size() : MAX_AXIS;
        for (size_t i = 0; i < minsize; i++) { val[i] = v[i]; }
    }
    RobJointVal(ZucPose Pose)
    {
        val[0] = Pose.tran.x;
        val[1] = Pose.tran.y;
        val[2] = Pose.tran.z;
        val[3] = Pose.a;
        val[4] = Pose.b;
        val[5] = Pose.c;
        val[6] = Pose.u;
    }
    void array_value(double v[MAX_AXIS])
    {
        for (size_t i = 0; i < MAX_AXIS; i++) { v[i] = val[i]; }
    }
    std::vector<double> vector_value()
    {
        std::vector<double> v;
        for (size_t i = 0; i < MAX_AXIS; i++) { v.push_back(val[i]); }
        return v;
    }
};

class AxisGroupPose
{
public:
    RobJointVal jpos;
    ZucPose pose = {};
    std::vector<double> pose_array() const
    {
        std::vector<double> res{pose.tran.x, pose.tran.y, pose.tran.z, pose.a, pose.b, pose.c};
        return res;
    }
    std::string pose_str() const
    {
        std::string res = std::to_string(pose.tran.x) + ", " + std::to_string(pose.tran.y) + ", " + std::to_string(pose.tran.z) + ", " +
                          std::to_string(pose.a) + ", " + std::to_string(pose.b) + ", " + std::to_string(pose.c);
        return res;
    }
};

typedef struct RobotDH
{
    RobJointVal alpha;
    RobJointVal a;
    RobJointVal d;
    RobJointVal joint_homeoff;
    std::string get_robot_dh_str() const
    {
        std::string str = "";
        str += "DH_A = :" + a.val_str() + "\n";
        str += "DH_D = :" + d.val_str() + "\n";
        str += "DH_ALPHA = :" + alpha.val_str() + "\n";
        str += "DH_OFFSET = :" + joint_homeoff.val_str() + "\n";
        return str;
    }
} RobotDH;

typedef struct SensorStatus
{
    int status;
    int errCode;
    double torque[6];      ///< torque value from sensor module
    double actTorque[6];   /// actual torque value from  motion module
    double realTorque[6];  /// always apply gravity and bias compensation
} SensorStatus;

typedef struct SensorConfig
{
    int commType;
    int torqsensorBrand;  //torque sensor brand 1 for ATI,2 for kungwei ,3 for yuli
    int mode;
} SensorConfig;

typedef struct SensorShm
{
    // controller --> sensor manager
    SensorConfig sensorConfig;
    // sensor manager --> controller
    SensorStatus sensorStatus;
    double torqueSensorFilter;
} SensorShm;

class RobTarget
{
public:
    RobTarget() { memset(&pose, 0, sizeof(ZucPose)); }
    RobTarget(ZucPose p) : pose(p) {}
    RobTarget(double* p)
    {
        pose.tran.x = p[0];
        pose.tran.y = p[1];
        pose.tran.z = p[2];
        pose.a = p[3];
        pose.b = p[4];
        pose.c = p[5];
    }
    ZucPose pose;
    RobJointVal ref_jpos;  //关节参考位置
};

struct MoveSpeed
{
    double vel;       //线速度
    double acc;       //线加速度
    double jerk;      //线加加速度
    double ori_vel;   //姿态速度
    double ori_acc;   //姿态加速度
    double ori_jerk;  //姿态加加速度
    double j_vel;     //关节速度
    double j_acc;     //关节加速度
    double j_jerk;    //关节加加速度
    MoveSpeed(double vel_, double acc_, double jerk_, double ori_vel_, double ori_acc_, double ori_jerk_, double j_vel_, double j_acc_, double j_jerk_)
        : vel(vel_), acc(acc_), jerk(jerk_), ori_vel(ori_vel_), ori_acc(ori_acc_), ori_jerk(ori_jerk_), j_vel(j_vel_), j_acc(j_acc_), j_jerk(j_jerk_)
    {
    }
    MoveSpeed(double vel_, double acc_, double jerk_, double ori_vel_, double ori_acc_, double ori_jerk_)
        : vel(vel_), acc(acc_), jerk(jerk_), ori_vel(ori_vel_), ori_acc(ori_acc_), ori_jerk(ori_jerk_)
    {
    }
    MoveSpeed(double j_vel_, double j_acc_, double j_jerk_) : j_vel(j_vel_), j_acc(j_acc_), j_jerk(j_jerk_) {}
};

class Traj_Input_Param
{
public:
    double ini_maxvel;
    int using_abc;
    int di_type;
    int di_index;
    int di_state;
    int planner_type;
    int is_jog;
    Traj_Input_Param(double ini_maxvel_, int using_abc_, int di_type_, int di_index_, int di_state_, int planner_type_, int is_jog_)
        : ini_maxvel(ini_maxvel_), using_abc(using_abc_), di_type(di_type_), di_index(di_index_), di_state(di_state_), planner_type(planner_type_),
          is_jog(is_jog_)
    {
    }
};

class Movc_Param : public Traj_Input_Param
{
public:
    Movc_Param(int is_jog_,
               double ini_maxvel_,
               int using_abc_,
               int di_type_,
               int di_index_,
               int di_state_,
               int planner_type_,
               double circle_count_,
               int circlemode_)
        : Traj_Input_Param(ini_maxvel_, using_abc_, di_type_, di_index_, di_state_, planner_type_, is_jog_), circle_count(circle_count_),
          circlemode(circlemode_)
    {
    }
    double circle_count;
    int circlemode;
};

class Movs_Param : public Traj_Input_Param
{
public:
    Movs_Param(double a0_[MAX_AXIS], double a1_[MAX_AXIS], double a2_[MAX_AXIS], double a3_[MAX_AXIS], double time_) : Traj_Input_Param(0, 0, 0, 0, 0, 0, 0)
    {
        //: Traj_Input_Param(0, 0, 0, 0, 0, 0, 0), a0(a0_), a1(a1_), a2(a2_), a3(a3_), time(time_)
        memcpy(a0, a0_, MAX_AXIS * sizeof(double));
        memcpy(a1, a1_, MAX_AXIS * sizeof(double));
        memcpy(a2, a2_, MAX_AXIS * sizeof(double));
        memcpy(a3, a3_, MAX_AXIS * sizeof(double));
        time = time_;
    }
    double a0[MAX_AXIS];
    double a1[MAX_AXIS];
    double a2[MAX_AXIS];
    double a3[MAX_AXIS];
    double time;
};

class MovjParam : public Traj_Input_Param
{
public:
    MovjParam(int is_jog_, int mvOpt_, double tol_, int executing_line_id_, int* end_cond_, int is_cpos)
        : Traj_Input_Param(0, 0, end_cond_[0], end_cond_[1], end_cond_[2], 0, is_jog_), type(0), indexrotary(0),
          jog_movj(mvOpt_, executing_line_id_, tol_, is_cpos)
    {
    }
    MovjParam(int is_jog_, int type_, double ini_maxvel_, int indexrotary_, int di_type_, int di_index_, int di_state_, int jointNum_, int planner_type_)
        : Traj_Input_Param(ini_maxvel_, 0, di_type_, di_index_, di_state_, 0, is_jog_), type(type_), indexrotary(indexrotary_), jog_movj(0, 0, 0, 0)
    {
    }
    struct jog
    {
        jog(int mvOpt_, double tol_, int executing_line_id_, int is_cpos_) : mvOpt(mvOpt_), tol(tol_), executing_line_id(executing_line_id_), is_cpos(is_cpos_)
        {
        }
        int mvOpt;
        double tol;
        int executing_line_id;
        int is_cpos;
    };
    int type;
    int indexrotary;

    struct jog jog_movj;
};

class MovLParam : public Traj_Input_Param
{
public:
    MovLParam(int is_jog_, int mvOpt_, double tol_, int executing_line_id_, int* end_cond_)
        : Traj_Input_Param(0, 0, end_cond_[0], end_cond_[1], end_cond_[2], 0, is_jog_), type(0), indexrotary(0), jog_movl(mvOpt_, executing_line_id_, tol_)
    {
    }
    MovLParam(int is_jog_, int type_, double ini_maxvel_, int indexrotary_, int using_abc_, int di_type_, int di_index_, int di_state_, int planner_type_)
        : Traj_Input_Param(ini_maxvel_, using_abc_, di_type_, di_index_, di_state_, planner_type_, is_jog_), type(type_), indexrotary(indexrotary_),
          jog_movl(0, 0, 0)
    {
    }
    struct jog
    {
        jog(int mvOpt_, double tol_, int executing_line_id_) : mvOpt(mvOpt_), tol(tol_), executing_line_id(executing_line_id_) {}
        int mvOpt;
        double tol;
        int executing_line_id;
    };
    int type;
    int indexrotary;
    struct jog jog_movl;
};
